
#include <BFixedRestraint>
#include "member_BFixedRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

#define member					(*(member_BFixedRestraint*)_ptr)
#define member_allocate()		_ptr = new member_BFixedRestraint(this)
#define member_release()		delete (member_BFixedRestraint*)_ptr

BFixedRestraint::BFixedRestraint()
{
	member_allocate();
}
BFixedRestraint::BFixedRestraint(BBody* bodyA, const BVector& pivotA) : BRestraint(bodyA, pivotA)
{
	member_allocate();
}
BFixedRestraint::BFixedRestraint(BBody* bodyA, BBody* bodyB) : BRestraint(bodyA, bodyB)
{
	member_allocate();
}
BFixedRestraint::BFixedRestraint(BBody* bodyA, const BVector& pivotA, BBody* bodyB, const BVector& pivotB) : BRestraint(bodyA, pivotA, bodyB, pivotB)
{
	member_allocate();
}
BFixedRestraint::~BFixedRestraint()
{
	member_release();
}

void BFixedRestraint::copy(const BRestraint* other)
{
	BRestraint::copy(other);
}
BRestraint* BFixedRestraint::clone() const
{
	BFixedRestraint* restraint = new BFixedRestraint();
	restraint->copy(this);
	return restraint;
}

bool BFixedRestraint::setRotation(BReal rx, BReal ry, BReal rz)
{
	member.rotation.set(rx, ry, rz);
	return true;
}
bool BFixedRestraint::setRotation(const BVector& rotation)
{
	member.rotation = rotation;
	return true;
}
const BVector& BFixedRestraint::rotation() const
{
	return member.rotation;
}

int BFixedRestraint::solverSize()
{
	member.calculateTransforms();
	int num = 0;
	for (int i = 0; i < 3; i++)
	{
		if (member.angularLimits[i].limited)
		{
			num++;
		}
	}
	for (int i = 0; i < 3; i++)
	{
		if (member.linearLimits[i].limited)
		{
			num++;
		}
	}
	return num;
}
void BFixedRestraint::soverItems(BSolverItem* solverItems, BReal stepTime)
{
	const BBody* bodyA = restraint_member(this)->bodyA;
	const BBody* bodyB = restraint_member(this)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	BVector pointA = matrixA * restraint_member(this)->pivotA;
	BVector pointB = matrixB * restraint_member(this)->pivotB;

	int row = 0;
	for (int i = 0; i < 3; i++)
	{
		RotLimit& limitMotor = member.angularLimits[i];
		if (limitMotor.limited)
		{
			BVector axis = member.calculatedAxis[i].normal();
			solverItems[row].torqueAxisA = axis;
			solverItems[row].torqueAxisB = -axis;
			solverItems[row].velocity = -limitMotor.limitError / stepTime * 0.2;
			solverItems[row].impulseLowerLimit = -REAL_MAX;
			solverItems[row].impulseUpperLimit = REAL_MAX;
			row++;
		}
	}

	BReal invMassA = body_member(bodyA)->invMass;
	BReal invMassB = body_member(bodyB)->invMass;
	bool hasStaticBody = (invMassA < REAL_EPSILON) || (invMassB < REAL_EPSILON);
	BReal miSum = invMassA + invMassB;
	BReal factA = invMassB / miSum;
	BReal factB = 1 - factA;

	for (int i = 0; i < 3; i++)
	{
		FixedLimit& limitMotor = member.linearLimits[i];
		if (limitMotor.limited)
		{
			BVector axis = matrixA.row(i);
			solverItems[row].contactNormalA = axis;
			solverItems[row].contactNormalB = -axis;

			BVector relB = pointB - matrixB.position();
			BVector projB = axis * relB.dot(axis);
			BVector orthoB = relB - projB;

			BVector relA = pointA - matrixA.position();
			BVector projA = axis * relA.dot(axis);
			BVector orthoA = relA - projA;

			BVector totalDist = projA - projB;
			relA = orthoA + totalDist * factA;
			relB = orthoB - totalDist * factB;
			BVector tmpA = relA.cross(axis);
			BVector tmpB = relB.cross(axis);
			if (hasStaticBody)
			{
				tmpA *= factA;
				tmpB *= factB;
			}
			solverItems[row].torqueAxisA = tmpA;
			solverItems[row].torqueAxisB = -tmpB;
			solverItems[row].velocity = limitMotor.limitError / stepTime * 0.2;
			solverItems[row].impulseLowerLimit = -REAL_MAX;
			solverItems[row].impulseUpperLimit = REAL_MAX;
			row++;
		}
	}
}

bool BFixedRestraint::query(const BString& text, BValue& value) const
{
	if (text == "debug-axis0")
	{
		value = member.debugAxis[0];
		return true;
	}
	if (text == "debug-axis1")
	{
		value = member.debugAxis[1];
		return true;
	}
	if (text == "debug-axis2")
	{
		value = member.debugAxis[2];
		return true;
	}
	return BRestraint::query(text, value);
}
